﻿using System;
using BulletSharp;
using SimFramework;
using btl.generic;
using System.Collections.Generic;
using System.Linq;

namespace Robot
{
    class Robot : Simulation
    {
        public static Random R = new Random();
        Vector3 eye = new Vector3(-10, 10, -20);
        Vector3 target = new Vector3(0, 5, -4);

        const int robotCount = 8;

        List<RobotModel> robots = new List<RobotModel>();
        Food[] foods = new Food[robotCount];
        float time = 0;
        float maxTime = 10;
        float bestDistance = int.MaxValue;
        float globalBestDistance = int.MaxValue;
        int iteration = 1;

        //Genetic algorithm
        GA geneticAlgorithm;

        protected override void OnInitialize()
        {
            Freelook.SetEyeTarget(eye, target);

            Graphics.SetFormText("Robot Simulation");
            SetText();

            DebugDrawMode = DebugDrawModes.DrawConstraintLimits | DebugDrawModes.DrawConstraints | DebugDrawModes.DrawWireframe;
        }

        protected void SetText()
        {
            float timeLeft = maxTime - time;
            Graphics.SetInfoText("Move using mouse and WASD+shift\n" +
                "F3 - Toggle debug\n" +
                "F11 - Toggle fullscreen\n" +
                "iteration: " + iteration + "\n" +
                "bestDistance: " + bestDistance.ToString("#,00") + "\\" + globalBestDistance.ToString("#,00") + "\n" +
                "timeLeft: " + timeLeft.ToString("#,00"));
        }

        protected override void OnInitializePhysics()
        {
            // collision configuration contains default setup for memory, collision setup
            CollisionConf = new DefaultCollisionConfiguration();
            Dispatcher = new CollisionDispatcher(CollisionConf);

            Broadphase = new DbvtBroadphase();

            World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, null, CollisionConf);
            World.Gravity = new Vector3(0, -10, 0);
            World.SetInternalTickCallback(MotorPreTickCallback, null, true);

            // create the ground
            BoxShape groundShape = new BoxShape(50, 0.5f, 50);
            CollisionShapes.Add(groundShape);
            CollisionObject ground = LocalCreateRigidBody(0, Matrix.Identity, groundShape);
            
            ground.Friction = 1;
            ground.UserObject = "Ground";


            if (geneticAlgorithm == null)
            {
                geneticAlgorithm = new GA(0.50, 0.05, robotCount, 2000);
                geneticAlgorithm.Elitism = true;
                geneticAlgorithm.FitnessFunction = FitnessFunction;
                int numWeightsInNN = RobotModel.NumLegs * 2;
                geneticAlgorithm.InitGenomes(numWeightsInNN);
            }

            initRobots();

            IsDebugDrawEnabled = true;
        }

        public void initRobots()
        {
            robots = new List<RobotModel>();
            var population = geneticAlgorithm.ThisGeneration;

            for (int i = 0; i < robotCount; i++)
            {
                foods[i] = new Food(new Vector3(25, 1f, i * 3), World);
                Vector3 position = new Vector3(0, 0.5f, i * 3);
                robots.Add(new RobotModel(World, position, foods[i]));

                robots[i].Genome = population[i];
            }
        }

        public override void OnUpdate()
        {
            //base.OnUpdateStep();
            SetText();
            base.OnUpdate();
        }

        private void evaluate()
        {
            bestDistance = float.MaxValue;
            foreach (RobotModel robot in robots)
            {
                robot.evaluate();
                if (robot.Distance < bestDistance)
                    bestDistance = (float)robot.Distance;
            }
            if (bestDistance < globalBestDistance)
                globalBestDistance = bestDistance;

            geneticAlgorithm.RankPopulation();
            geneticAlgorithm.CreateNextGeneration();

            //new generation
            time = 0;
            foreach (RobotModel robot in robots)
            {
                robot.Remove(World);
            }
            foreach (Food food in foods)
            {
                food.Remove(World);
            }
            initRobots();
            iteration++;
        }

        void MotorPreTickCallback(DynamicsWorld world, float timeStep)
        {
            foreach (RobotModel robot in robots)
            {
                robot.Update(timeStep);
            }
            time += timeStep;
            if (time > maxTime)
            {
                evaluate();
            }
        }

        double FitnessFunction(Genome genome)
        {
            for (int i = 0; i < robotCount; i++)
            {
                if (robots[i].Genome == genome)
                {
                    return robots[i].Fitness;
                }
            }
            return int.MaxValue;
        }

        [STAThread]
        static void Main()
        {
            using (Robot game = new Robot())
            {
                LibraryManager.Initialize(game);
            }
        }


    }
}
